#include "Settings.h"
#include "Converter.h"
#include "System.h"

#include <opencv2/core/persistence.hpp>
#include <opencv2/core/eigen.hpp>

#include <iostream>

using namespace std;

namespace msf
{

template <>
float Settings::readParameter<float>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required)
{
      cv::FileNode node = fSettings[name];
      if (node.empty())
      {
            if (required)
            {
                  std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
                  exit(-1);
            }
            else
            {
                  std::cerr << name << " optional parameter does not exist..." << std::endl;
                  found = false;
                  return 0.0f;
            }
      }
      else if (!node.isReal())
      {
            std::cerr << name << " parameter must be a real number, aborting..." << std::endl;
            exit(-1);
      }
      else
      {
            found = true;
            return node.real();
      }
}

template <>
int Settings::readParameter<int>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required)
{
      cv::FileNode node = fSettings[name];
      if (node.empty())
      {
            if (required)
            {
                  std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
                  exit(-1);
            }
            else
            {
                  std::cerr << name << " optional parameter does not exist..." << std::endl;
                  found = false;
                  return 0;
            }
      }
      else if (!node.isInt())
      {
            std::cerr << name << " parameter must be an integer number, aborting..." << std::endl;
            exit(-1);
      }
      else
      {
            found = true;
            return node.operator int();
      }
}

template <>
string Settings::readParameter<string>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required)
{
      cv::FileNode node = fSettings[name];
      if (node.empty())
      {
            if (required)
            {
                  std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
                  exit(-1);
            }
            else
            {
                  std::cerr << name << " optional parameter does not exist..." << std::endl;
                  found = false;
                  return string();
            }
      }
      else if (!node.isString())
      {
            std::cerr << name << " parameter must be a string, aborting..." << std::endl;
            exit(-1);
      }
      else
      {
            found = true;
            return node.string();
      }
}

template <>
cv::Mat Settings::readParameter<cv::Mat>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required)
{
      cv::FileNode node = fSettings[name];
      if (node.empty())
      {
            if (required)
            {
                  std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
                  exit(-1);
            }
            else
            {
                  std::cerr << name << " optional parameter does not exist..." << std::endl;
                  found = false;
                  return cv::Mat();
            }
      }
      else
      {
            found = true;
            return node.mat();
      }
}

Settings::Settings(const std::string &configFile, const int &sensor)
{
      sensor_ = sensor;

      //打开配置文件
      cv::FileStorage fSettings(configFile, cv::FileStorage::READ);
      if (!fSettings.isOpened())
      {
            cerr << "[ERROR]: could not open configuration file at: " << configFile << endl;
            cerr << "Aborting..." << endl;

            exit(-1);
      }
      else
      {
            cout << "Loading settings from " << configFile << endl;
      }

      if (sensor_ == System::IMU_ODOM)
      {
            readIMU(fSettings);
            cout << "\t-Loaded IMU calibration" << endl;
      }

      readOdom(fSettings);
      cout << "\t-Loaded Odom info" << endl;

      readViewer(fSettings);
      cout << "\t-Loaded viewer settings" << endl;

      cout << "----------------------------------" << endl;
}

void Settings::readOdom(cv::FileStorage &fSettings)
{
      bool found;

      // Read odom model
      string odomModel = readParameter<string>(fSettings, "Odom.type", found);

      if(odomModel == "Two_Diff")
      {
            odomType_ = Two_Diff;

            // Read odom parameters
            WheelRadius_ = readParameter<float>(fSettings, "Odom.WheelRadius", found);
            ReductionRatio_ = readParameter<float>(fSettings, "Odom.ReductionRatio", found);
            BodyRadius_ = readParameter<float>(fSettings, "Odom.BodyRadius", found);
            Frequency_ = readParameter<int>(fSettings, "Odom.Frequency", found);

      }
      else
      {
            cerr << "Settings: Error: " << odomModel << " not known" << endl;
            exit(-1);
      }

      fps_ = readParameter<int>(fSettings, "Odom.Frequency", found);
}

void Settings::readIMU(cv::FileStorage &fSettings)
{
    bool found;
    noiseGyro_ = readParameter<float>(fSettings, "IMU.NoiseGyro", found);
    noiseAcc_ = readParameter<float>(fSettings, "IMU.NoiseAcc", found);
    gyroWalk_ = readParameter<float>(fSettings, "IMU.GyroWalk", found);
    accWalk_ = readParameter<float>(fSettings, "IMU.AccWalk", found);
    imuFrequency_ = readParameter<float>(fSettings, "IMU.Frequency", found);

    cv::Mat cvTbo = readParameter<cv::Mat>(fSettings, "IMU.T_b_o", found);
    Tbo_ = Converter::toSophus(cvTbo);

}

void Settings::readViewer(cv::FileStorage &fSettings)
{
    bool found;

    keyPoseFrameSize_ = readParameter<float>(fSettings, "Viewer.KeyFrameSize", found);
    keyPoseFrameLineWidth_ = readParameter<float>(fSettings, "Viewer.KeyFrameLineWidth", found);
    graphLineWidth_ = readParameter<float>(fSettings, "Viewer.GraphLineWidth", found);
    pointSize_ = readParameter<float>(fSettings, "Viewer.PointSize", found);
    odomSize_ = readParameter<float>(fSettings, "Viewer.OdomSize", found);
    odomLineWidth_ = readParameter<float>(fSettings, "Viewer.OdomLineWidth", found);
    viewPointX_ = readParameter<float>(fSettings, "Viewer.ViewpointX", found);
    viewPointY_ = readParameter<float>(fSettings, "Viewer.ViewpointY", found);
    viewPointZ_ = readParameter<float>(fSettings, "Viewer.ViewpointZ", found);
    viewPointF_ = readParameter<float>(fSettings, "Viewer.ViewpointF", found);
    //imageViewerScale_ = readParameter<float>(fSettings, "Viewer.imageViewScale", found, false);

    //if (!found)
        //imageViewerScale_ = 1.0f;
}

}